/*
 * Test program for the ball-catching part
 */

# include <init_systems.h>
# include <log.h>
# include <time_tools.h>
# include "robo_control.h"
# include "ball_catcher.h"
# include "buttons_reader.h"

//130.237.226.221

using namespace kthrobot;
using namespace std;
using namespace Group5;

int main (int argc, char **argv) {

    Init::RegisterCommon();
    RRCon::Register();
    if ( !Init::Systems(argc, argv) ) return 1;


    {
//        bool *button_pressed;
        ButtonsReader buttons_reader;
        buttons_reader.Start();

        // Waiting for a press on the center button to start
//        button_pressed = buttons_reader.WaitForButton();

        // Launching the task
        Robo_Control motor1("Motor1",0), motor2("Motor2",3);
        BallCatcher ball_catcher(&motor1, &motor2);
        motor1.Start();
        motor2.Start();
        ball_catcher.Start();

        // Waiting for a press a button to stop
 //       button_pressed = buttons_reader.WaitForButton();
 //       Init::SignalShutdown(); // Asks all tasks to terminate
    }

    Init::Shutdown();
    return 0;
}
